ParticleFilter = function (gs) {
	this.shouldUpdate = false;			
	this.resampleCount = 0;
	
	this.particles = [];	
	for (var i = 0; i < NUMBER_OF_PARTICLES; i++) {
		var particle = createParticle(gs);
		gs.addEntity(particle);
		this.particles.push(particle);
	}
	
	gs.addEntity(this);
	
	this.keyHeld_38 = function () {		
		this.shouldUpdate = true;
	}
	
	this.updateFilter = function() {
		var weights = getWeights(this.particles);
		var newParticles = resample(this.particles, weights);
		for (var i = 0; i < this.particles.length; i++) {
			gs.delEntity(this.particles[i]);
			gs.addEntity(newParticles[i]);
			newParticles[i].keyHeld_38();
		}
		this.particles = newParticles;
	}
	
	this.draw = function() {
		if (this.shouldUpdate && this.resampleCount++ % RESAMPLE_DELAY === 0) {
			this.updateFilter();
			this.shouldUpdate = false;
		}		
	}	
}

function createParticle(gs) {
	var x = translateHorizontalCoordinate(gs.random(0, 100));
	var y = translateVerticalCoordinate(gs.random(0, 100));
	var h = gs.random(0, 2 * Math.PI);
	return new Robot(x, y, h, 0, true);
}

function getWeights(particles) {
	var z = ROBOT.getMeasurement();
	var weights = [];
	for (var i = 0; i < particles.length; i++) {
		weights.push(z.probabilityFor(particles[i]));
	}
	return weights;
}

function resample(particles, weights) {
	var newParticles = [];
    var index = Math.floor(Math.random() * weights.length);
    var beta = 0;
    var mw = Math.max.apply(Math, weights);
    for (var i= 0; i < weights.length; i++) {
        beta += Math.random() * 2 * mw;
        while (beta > weights[index]){
            beta -= weights[index];
            index = (index + 1) % weights.length;
		}
		var newParticle = copyParticle(particles[index]);
        newParticles.push(newParticle);
	}
	return newParticles;
}

function copyParticle(p) {
	return new Robot(p.x, p.y, p.angle, p.speed, p.isParticle);
}